#!/usr/bin/python3
import roslibpy
import numpy as np
import spatialmath.base as smb
import math
import time

class PartAssembleClient:
    def __init__(self,type="simulating", serverip="127.0.0.1"):
        self.type = type
        self.typeSchemeMapping = {}
        self.typeSchemeMapping['simulating'] = self.wholeSimulationFunction
        self.typeSchemeMapping['hardwareInLoop'] = self.hardwareInLoopFunction
        self.typeSchemeMapping['calibration'] = self.calibrationSystem
        self.rosServer = None
        self.serverip = serverip

    def getValidRosServer(self):
        if self.rosServer is None:
            self.rosServer = roslibpy.Ros(host = self.serverip, port=9090)
            self.rosServer.run()
        return self.rosServer

    def close(self):
        if self.rosServer is not None:
            self.rosServer.terminate()
            self.rosServer = None

    def run(self):
        self.typeSchemeMapping[self.type]()

    def wholeSimulationFunction(self):
        rosServer = self.getValidRosServer()
        pub = roslibpy.Topic(rosServer,'/zhongduan/holding_state','std_msgs/Float64MultiArray')
        pub.publish({'data':[6.1855,0.0,0.0,0.0,0.0,0.0]})
        time.sleep(1.0)

        service = roslibpy.Service(rosServer, '/zhongduan/get_key_feature_measure', 'zhongduan/GetKeyFeatureMeasure')

        request = roslibpy.ServiceRequest({'key_feature_name':{'data':"dingweikong"}})
        print('Calling service...')
        result = service.call(request)
        print(result)
        measured_points = result

        service2 = roslibpy.Service(rosServer, '/zhongduan/get_zhujian_pan_pose', 'zhongduan/GetZhujianPanPose')
        request = roslibpy.ServiceRequest(measured_points)
        print('Calling service...')
        result = service2.call(request)
        print(result)

    def hardwareInLoopFunction(self):
        pass

    def calibrationSystem(self):
        rosServer = self.getValidRosServer()
        pub = roslibpy.Topic(rosServer,'/zhongduan/holding_state','std_msgs/Float64MultiArray')
        pub.publish({'data':[6.1855,0.0,0.0,0.0,0.0,0.0]})
        time.sleep(1.0)

        service = roslibpy.Service(rosServer, '/fine_machining/calibrate_stewart', 'plane_part_stewart_driver/CalibrateStewart')

        request = roslibpy.ServiceRequest({
            'worldBottomRel':{'data':[6620, 0.0, 3016, 180.0, 0.0, 180.0]},
            'bottomPlatformRel':{'data':[0.0,0.0,600.0,0.0,0.0,0.0]},
            'platformModulatingRel':{'data':[-6, 0.0, 45, 180.0, 0.0, 180.0]}
            })

        print('Calling service...')
        result = service.call(request)
        print(result)
        measured_points = result

if __name__=='__main__':
    scheme = PartAssembleClient('calibration')
    scheme.run()
